/**
 * @file	mouse_controller.c
 * @author	Mitch, David
 * @brief	Mouse Controller. Creates 3 tasks: Sending, Receiving, and Control.
 * 			Sending and Receiving tasks are system tasks, and control task is BRR.
 * 			Control task is the brain of the mouse. It polls sensors to detect the cat,
 * 			and will execute escaping commands accordingly.
 */

#include "header/common.h"
#include "header/os.h"
#include "roomba/roomba.h"
#include "roomba/roomba_sci.h"
#include "uart/uart.h"
#include "radio/radio.h"
#include "header/sensor.h"
#include "header/mouse_controller.h"
#include <stdlib.h>

#ifdef MOUSE_CONTROLLER

/* RTOS requires these PPP arrays definitions */
const unsigned char PPP[] = {};
const unsigned int PT = sizeof(PPP)/2;

/** Event variable that is signalled or waited upon for sending through radio */
EVENT *sendEvent;

/** event variable that is signalled or waited upon radio receive */
EVENT *receiveEvent;

/** flag to trigger game over */
int game_over_flag = 0;

/** flag to trigger sending event */
int send_flag = 0;

/** Radio variables */
uint8_t station1_addr[5] = { 0x11, 0x11, 0x11, 0x11, 0x11 };
radiopacket_t packet;
volatile uint8_t rxflag = 0; // Radio Receive Flag
uint8_t cat_addr[5] = { 0x12, 0x12, 0x12, 0x12, 0x12 }; //cat address
radiopacket_t command_packet; //packet to be sent containing movement command

/** Sensor direction */
IR_Direction_t direction = IR_NONE;

/**
 * @brief	LED initializaton function. Initialize ports PD4 to PD7 to use LEDs.
 */
void init_LED(void) {
	DDRD = (1<<PORTD4) | (1<<PORTD5) | (1<<PORTD6) | (1<<PORTD7);
}

/**
 * @brief	Initialize radio.
 */
void init_radio(void) {
	Radio_Init();
	Radio_Configure_Rx(RADIO_PIPE_0, station1_addr, ENABLE);
	Radio_Configure(RADIO_2MBPS, RADIO_HIGHEST_POWER);
	Radio_Set_Tx_Addr(cat_addr);
	set_roomba_command(&command_packet, POWER);
}

/*
 * @brief Prepare roomba command packet.
 *
 * @param packet a packet to be sent to Roomba
 * @param command a command to be sent to Roomba
 * */
void set_roomba_command(radiopacket_t *packet, uint8_t command) {
	//copy sender address to packet
	memcpy(command_packet.payload.command.sender_address, cat_addr, 5);
	packet->type = COMMAND;
	packet->payload.command.command = command;
}

/**
 * @brief	Roomba Driving function. Parameters are velocity and radius.
 */
void roomba_Drive( int16_t velocity, int16_t radius )
{
	uart_putchar(DRIVE);
	uart_putchar(HIGH_BYTE(velocity));
	uart_putchar(LOW_BYTE(velocity));
	uart_putchar(HIGH_BYTE(radius));
	uart_putchar(LOW_BYTE(radius));
}

/**
 * @brief	Search for n ticks.
 */
void search_n_ticks(int n) {
	unsigned int start = Now();

	for (;;) {
		/** n x 5 milliseconds = number of milliseconds searching */
		if ((Now() - start) >= n) {
			break;
		}
	}
}

/**
 * @brief	Get bumper status. Return 0 if not activated, otherwise 1.
 */
int get_bumper_status() {
	/** set roomba to sensor data mode */
	uart_putchar(SENSORS);

	/** ask roomba for external sensor data */
	uart_putchar(1);

	/** retrieve first byte corresponding to bumper status */
	while (uart_bytes_received() != 10);

	/** store bumper status information in local variable */
	uint8_t bumper_status = uart_get_byte(0);

	/** reset received when done */
	uart_reset_receive();

	/** if bumper is not activated, return 0 */
	if (bumper_status == 0) {
		return 0;
	} else { /** if bumper is activate, return 1 */
		return 1;
	}
}

/**
 * @brief	return max value between a and b.
 */
int max(int a, int b) {
	if (a >= b) {
		return a;
	}
	else {
		return b;
	}
}

/**
 * @brief	Sample sensor 10 times to make sure north, east or west sensor is active.
 */
int is_sensor_north(void) {
	int i=0;
	int north = 0;
	int south = 0;
	int result = 0;

	for (i=0; i<10; i++) {
		direction = get_current_direction();

		if (direction == IR_NORTH || direction == IR_EAST || direction == IR_WEST) {
			north = north + 1;
		}
		else if (direction == IR_SOUTH) {
			south = south + 1;
		}
	}

	result = max(north,south);

	if (result == north) {
		return 1;
	}
	else {
		return 0;
	}
}

/**
 * @brief	Radio Send task.
 */
void radio_tx(void) {
	for (;;) {
		if (send_flag) {
			send_flag = 0;
			Radio_Transmit(&command_packet, RADIO_WAIT_FOR_TX);
		}
		Event_Wait(sendEvent);
	}
}

/**
 * @brief	Radio Receive task.
 */
void radio_receive_task(void) {
	uint8_t bumper_status = 0;
	uint8_t sensor_status = 0;

	for (;;) {
		if (rxflag) {
			roomba_Drive(0,0);
			_delay_ms(20);

			if (Radio_Receive(&packet) != RADIO_RX_MORE_PACKETS) {
				rxflag = 0;
			}
			if (packet.payload.command.command != POWER) {
				/** Set radio's destination address to be the remote station's address */
				Radio_Set_Tx_Addr(packet.payload.command.sender_address);

				uart_putchar(SENSORS);
				uart_putchar(1);
				while (uart_bytes_received() <= 0);
				bumper_status = uart_get_byte(0);
				uart_reset_receive();

				/** Set bumper status info into packet */
				packet.payload.sensors.sensors.bumps_wheeldrops = bumper_status;

				/** get sensor status */
				if (is_sensor_north()) {
					sensor_status = 1;
					packet.payload.sensors.sensors.wall = 1;
				}
				else {
					sensor_status = 0;
					packet.payload.sensors.sensors.wall = 0;
				}

				/** Send the sensor packet back to the cat */
				packet.type = SENSOR_DATA;

				/** Transmit packet back */
				Radio_Transmit(&packet,RADIO_RETURN_ON_TX);
				if (bumper_status != 0 && sensor_status != 0) {
					game_over_flag = 1;
				}
			}
		}
		Event_Wait(receiveEvent);
	}
}

/**
 * @brief	Control task function. Roomba randomly moves forward, backward, turn left or turn right.
 */
void control_task(void) {
	for (;;) {
		unsigned int start = Now();

		for (;;) {
			if ((Now() - start) >= 6000) {
				roomba_Drive(0,0);
				send_flag = 1;
				PORTD ^=_BV(PD6); //green
				Event_Signal(sendEvent);
				Task_Terminate();
				break;
			} else {
				if (game_over_flag) {
					Task_Terminate();
				}
				direction = get_current_direction();
				if (direction == IR_WEST) {
					roomba_Drive(50, 0xFFFF);
					search_n_ticks(200);
					roomba_Drive(50, 0x8000);
				}
				else if (direction == IR_EAST) {
					roomba_Drive(50, 0x0001);
					search_n_ticks(200);
					roomba_Drive(50, 0x8000);
				}
				else if (direction == IR_SOUTH) {
					roomba_Drive(50, 0x8000);
				}
				else if (direction == IR_NORTH) {
					roomba_Drive(0xFFCE, 0x8000);
				} else {
				}
				search_n_ticks(200);
			}
		}
	}
}

/**
 * @brief	Mouse Controller Main function. Instantiate system and BRR tasks.
 */
int main (void) {

	/** Set clock speed to 8MHz */
	clock8MHz();

	/** disable interrupts */
	cli();

	/** initialize Radio */
	init_radio();

	/** initialize LEDs */
	init_LED();

	/** initialize Roomba */
	Roomba_Init();

	/** Initialize events */
	sendEvent = Event_Init();
	receiveEvent = Event_Init();

	/** enable interrupts */
	sei();

	/** create a system task for receiving radio */
	Task_Create(radio_receive_task,1,SYSTEM,1);

	/** create a system task for receiving radio */
	Task_Create(radio_tx,2,SYSTEM,2);

	/** Create a BRR task for searching */
	Task_Create(control_task, 0, BRR, 0);

	return 0;
}

/**
 * Interrupt function for radio rx.
 */
void radio_rxhandler(uint8_t pipenumber) {
	rxflag = 1;
	Event_Signal(receiveEvent);
}

#endif
